// -*- coding: utf-8 -*-
// 假数据发布器，用于测试MCU控制系统
// 
// 功能说明:
// 1. 监听主控节点发布的state状态
// 2. 收到特定状态时同时进行两种数据发布:
//    - 通过McuDataSender向串口发送JSON数据（模拟真实MCU通信）
//    - 通过McuDataSubscriber直接在本地ROS topic发布数据（内环自收发测试）
// 3. 支持状态:
//    - wait_for_pole1: 发布杆1坐标、rog=1.0、switch=to1
//    - wait_for_pole2: 发布rog=2.0、switch=to2
// 
// 内环自收发机制:
// - 不依赖真实的串口MCU硬件
// - 主控节点可以直接接收到本地发布的数据进行测试
// - 同时保持与真实系统相同的数据格式和接口
#include "mcu_data_sender.h"
#include "mcu_data_subscriber.h"
#include <ros/ros.h>
#include <iostream>
#include <string>

int main(int argc, char **argv)
{
    std::setlocale(LC_ALL, "zh_CN.UTF-8");

    ros::init(argc, argv, "fake_data_sender");
    ros::NodeHandle nh;
    
    ROS_INFO("=== 假数据发布器启动 ===");
    ROS_INFO("模式: 双重发布（串口 + 本地ROS topic）");
    ROS_INFO("内环自收发: 无需真实MCU硬件即可测试完整流程");
    
    // 初始化MCU数据发送器和订阅器
    McuDataSender mcuSender(nh);
    McuDataSubscriber mcuSubscriber(nh);
    
    // 等待一段时间让订阅器和发布器初始化完成
    ROS_INFO("等待McuDataSubscriber初始化完成...");
    ros::Duration(1.0).sleep();
    
    ROS_INFO("初始化完成，所有发布器和订阅器已准备就绪");
    
    ros::Rate rate(5.0); // 5Hz
    
    std::string lastState = "";
    bool pole1_sent = false;
    bool pole2_sent = false;
    
    ROS_INFO("等待接收状态信息...");
    ROS_INFO("当状态为 'wait_for_pole1' 时，将发送杆1数据");
    ROS_INFO("当状态为 'wait_for_pole2' 时，将发送杆2数据");

    mcuSubscriber.publishDoubleData("basic", 1.0);
    mcuSubscriber.publishDoubleData("rog", 1.0);

    while (ros::ok()) {
        ros::spinOnce();
        
        // 获取当前状态
        std::string currentState = mcuSubscriber.getStringValue("state");
        
        // 检测状态变化
        if (currentState != lastState && !currentState.empty()) {
            ROS_INFO("检测到状态变化: %s -> %s", lastState.c_str(), currentState.c_str());
            lastState = currentState;
            
            // 重置发送标志当状态改变时
            if (currentState != "wait_for_pole1") {
                pole1_sent = false;
            }
            if (currentState != "wait_for_pole2") {
                pole2_sent = false;
            }
        }
        
        // 当状态为wait_for_pole1时，发送杆1相关数据
        if (currentState == "wait_for_pole1" && !pole1_sent) {
            
            bool success = true;
            // success &= mcuSubscriber.publishDoubleData("pole1_x", 1.0);
            // ros::Duration(0.05).sleep();
            // success &= mcuSubscriber.publishDoubleData("pole1_y", -0.5);
            // ros::Duration(0.05).sleep();
            // success &= mcuSubscriber.publishDoubleData("pole2_x", 3.0);
            // ros::Duration(0.05).sleep();
            // success &= mcuSubscriber.publishDoubleData("pole2_y", -2.5);
            // ros::Duration(0.05).sleep();
            success &= mcuSubscriber.publishDoubleData("rog", 1.0);
            ros::Duration(0.05).sleep();
            // success &= mcuSubscriber.publishStringData("switch", "to1");
            
            if (success) {
                ROS_INFO("✓ 本地ROS topic发布成功");
            } else {
                ROS_WARN("✗ 本地ROS topic发布部分失败");
            }
            
            ROS_INFO("已发送: pole1_x=1.0, pole1_y=-0.5, pole2_x=3.0, pole2_y=-2.5, rog=1.0, switch=to1");
            ROS_INFO("数据已通过串口发送 + 本地ROS topic发布");
            pole1_sent = true;
        }
        
        // 当状态为wait_for_pole2时，发送杆2相关数据
        else if (currentState == "wait_for_pole2" && !pole2_sent) {
            ROS_WARN("=== 发送杆2假数据（串口+本地发布） ===");
            
            // 通过串口发送新的rog值（绿杆=2.0）
            mcuSender.sendJsonKeyValue("rog", 2.0);
            ros::Duration(0.05).sleep();
            mcuSender.sendJsonString("switch", "to2");
            ros::Duration(0.1).sleep();
            
            // 同时通过本地ROS topic发布（内环自收发）
            ROS_INFO("本地发布杆2数据到ROS topics...");
            
            bool success = true;
            success &= mcuSubscriber.publishDoubleData("rog", 2.0);
            ros::Duration(0.05).sleep();
            success &= mcuSubscriber.publishStringData("switch", "to2");
            
            if (success) {
                ROS_INFO("✓ 本地ROS topic发布成功");
            } else {
                ROS_WARN("✗ 本地ROS topic发布部分失败");
            }
            
            ROS_INFO("已发送: rog=2.0, switch=to2");
            ROS_INFO("数据已通过串口发送 + 本地ROS topic发布");
            pole2_sent = true;
        }
        
        // 显示当前状态信息
        if (!currentState.empty()) {
            ROS_INFO_THROTTLE(5.0, "当前状态: %s", currentState.c_str());
        } else {
            ROS_INFO_THROTTLE(5.0, "等待状态信息...");
        }
        
        rate.sleep();
    }
    
    ROS_INFO("假数据发布器退出");
    return 0;
}
